
#include "serialization/asn1.h"

//	#include "nrf_log.h"
#include "t1.h"
#include <stddef.h>
#include <stdint.h>
#include "Debug.h"

asn1_len_t asn1_get_len(uint8_t *payload) {
    asn1_tlv_len_t *len_byte = ((asn1_tlv_len_t *)(payload + 1));
    if (len_byte->form == SHORT) {
        return len_byte->len;
    }
    if (len_byte->len > 2) {
        LOG_E("Unsupported length");
    }
    asn1_len_t len = 0;
    for (int8_t i = 0; i < len_byte->len; i++) {
        len |= (asn1_len_t)(payload[2 + i]) << (8 * ((len_byte->len) - 1 - i));
    }
    return len;
}

asn1_val_t asn1_get_value(uint8_t *payload) {
    asn1_val_t p_value = payload + 2;
    asn1_tlv_len_t *len_byte = ((asn1_tlv_len_t *)(payload + 1));
    if (len_byte->form == LONG) {
        p_value += len_byte->len;
    }

    return p_value;
}

asn1_tlv_t asn1_unwrap(uint8_t *payload) {
    asn1_tlv_t tlv = {.tag = (asn1_tag_t)payload[0],
                      .len = (asn1_len_t)asn1_get_len(payload),
                      .value = (asn1_val_t)asn1_get_value(payload)};

    return tlv;
}

void /*t1_inf_t*/ asn1_i2c_unwrap(t1_inf_t *i2c_apdu, t1_inf_t *dest, uint16_t /*t1_len_t*/ *len) {
    *len = 0;
    asn1_tlv_t i2c_command = asn1_unwrap(i2c_apdu->payload);

    asn1_tlv_t i2c_header = asn1_unwrap(i2c_command.value);
    memcpy(&dest->header, i2c_header.value, i2c_header.len);
    *len += i2c_header.len;

    asn1_tlv_t i2c_bus_address = asn1_unwrap(i2c_header.value + i2c_header.len);
    asn1_tlv_t i2c_payload = asn1_unwrap(i2c_bus_address.value + i2c_bus_address.len);
    memcpy(dest->payload, &i2c_payload.tag.raw, sizeof(uint8_t));
    memcpy(dest->payload + 1, &i2c_payload.len, sizeof(uint8_t));
    memcpy(dest->payload + 2, i2c_payload.value, i2c_payload.len);
    *len += i2c_payload.len + 2;
}

asn1_builder_t *asn1_setLength(asn1_builder_t *self, asn1_len_t length) {
    self->payload[self->dataPtr++] = 0x82;
    self->payload[self->dataPtr++] = (length >> 8) & 0x00FF;
    self->payload[self->dataPtr++] = length & 0x00FF;
    return self;
}

asn1_builder_t *asn1_openTag(asn1_builder_t *self, tag_t tag) {
    self->tagPtrs[self->currentTag++] = self->dataPtr;
    self->payload[self->dataPtr++] = (uint8_t)tag.constructed;
    self->setLength(self, 0);
    return self;
}

asn1_builder_t *asn1_closeTag(asn1_builder_t *self) {
    uint8_t tagIdx = self->tagPtrs[self->currentTag - 1];
    uint8_t const lenOffset = 4;
    uint16_t len = self->dataPtr - (tagIdx + lenOffset);
    self->payload[tagIdx + 2] = (len >> 8) & 0x00FF;
    self->payload[tagIdx + 3] = len & 0x00FF;
    self->currentTag--;
    return self;
}

asn1_builder_t *asn1_closeAllTags(asn1_builder_t *self) {
    while (self->currentTag != 0) {
        self->closeTag(self);
    }
    return self;
}

asn1_builder_t *asn1_create(asn1_builder_t *self, uint8_t value[], uint16_t len) {
    memcpy(self->payload + self->dataPtr, value, len);
    self->dataPtr += len;
    return self;
}

